
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Multicopter.h
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <common/gp_math/gp_mathlib.h>
#include "Dynamical.h"
#include "Battery.h"
#include "Land.h"
#include "Aircraft.h"
/*-----------------------------------macro------------------------------------*/
#define        FG_MAX_ENGINES  (4)
#define        FG_MAX_WHEELS   (3)
#define        FG_MAX_TANKS    (4)

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    uint8_t _servo;          //对应pwm通道
    float    _angle;         //位置角
    float    _yaw_factor;    //反扭矩与偏航运动的关系
    uint8_t _channel;        //对应电机标号

}Motor_Frame;

/*
typedef struct
{
    uint16_t servos[16];
    /*struct
    {
        float speed;
        float dir;
        float turbulence;
        float dir_z;
    }wind;
    f//loat 
}Sitl_Input;*/

typedef struct
{
    float constrols[16];
    uint8_t mode;
}Sim_In;

typedef struct
{
    float mass;        //飞行器质量
    float uavR;        //半径
    
    //三轴转动惯量
    float jxx;
    float jyy;
    float jzz;

    //电机参数
    float kv;                //KV值(rpm/volt)
    float no_load_current;    //
    float resistance;        //内阻 欧姆
    float esc_resistance;  //电调内阻    欧姆
    float max_current;        //最大电流

    float pwmMin;
    float pwmMax;
    float spin_min;
    float spin_max;

    //电池参数
    float max_voltage;        //最大电压（volts）
    float batt_resistance;    //内阻  （ohms）
    float capacity;            //容量 （ah）

    //螺旋桨参数
    float diameter;
    float pitch;
    float prop_mass;
    float Cm;                //扭矩系数
    float Ct;                //拉力系数
    float intertia;            //螺旋桨转动惯量
    //环境参数
    float TempC;
    float Damp_Cd;            //阻力参数
    Vector3f_t Damp_CCm;
}Mode;


typedef struct
{
    AirCraft air_craft;
    char* name;
    uint8_t num_motors;
    Motor_Frame* motor_frame;
    Motor*  pMotor;
    Prop    prop;
    //模型参数
    Mode       mode;
    Battery    battery;
    Land       land;
    Vector3f_t Force;
    Vector3f_t Torque;
}Frame;


typedef struct
{
    uint32_t version;
    uint32_t padding;

    //位置信息
    double longitude;
    double latitude;
    double altitude;
    float agl;
    float phi;
    float theta;
    float psi;
    float alpha;
    float beta;

    //速度信息
    float phidot;
    float thetadot;
    float psidot;
    float vcas;
    float climb_rate;
    float v_north;              // north velocity in local/body frame, fps
    float v_east;               // east velocity in local/body frame, fps
    float v_down;               // down/vertical velocity in local/body frame, fps
    float v_body_u;             // ECEF velocity in body axis
    float v_body_v;             // ECEF velocity in body axis
    float v_body_w;             // ECEF velocity in body axis 

                                //加速度
    float A_X_pilot;
    float A_Y_pilot;
    float A_Z_pilot;

    float stall_warning;
    float slip_deg;

    //动力系统
    uint32_t num_engines;
    uint32_t eng_state[FG_MAX_ENGINES];// Engine state (off, cranking, running)
    float rpm[FG_MAX_ENGINES];       // Engine RPM rev/min
    float fuel_flow[FG_MAX_ENGINES]; // Fuel flow gallons/hr
    float fuel_px[FG_MAX_ENGINES];   // Fuel pressure psi
    float egt[FG_MAX_ENGINES];       // Exhuast gas temp deg F
    float cht[FG_MAX_ENGINES];       // Cylinder head temp deg F
    float mp_osi[FG_MAX_ENGINES];    // Manifold pressure
    float tit[FG_MAX_ENGINES];       // Turbine Inlet Temperature
    float oil_temp[FG_MAX_ENGINES];  // Oil temp deg F
    float oil_px[FG_MAX_ENGINES];    // Oil pressure psi

    uint32_t num_tanks;    // Max number of fuel tanks
    float fuel_quantity[FG_MAX_TANKS];

    uint32_t num_wheels;
    uint32_t wow[FG_MAX_WHEELS];
    float gear_pos[FG_MAX_WHEELS];
    float gear_steer[FG_MAX_WHEELS];
    float gear_compression[FG_MAX_WHEELS];

    uint32_t cur_time;           // current unix time
                                 // FIXME: make this uint64_t before 2038
    int32_t warp;                // offset in seconds to unix time
    float visibility;

    // Control surface positions (normalized values)
    float elevator;
    float elevator_trim_tab;
    float left_flap;
    float right_flap;
    float left_aileron;
    float right_aileron;
    float rudder;
    float nose_wheel;
    float speedbrake;
    float spoilers;
}FDM;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void MultiCopter_Init();
Frame* Multi_GetHandle(void);
void Multi_RunMain(float dt);
void Fdm_packed(char* fdm_buff, int* len);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif

